%%%  PosControl_HIL
clear
% path( './icon/',path);
% load vehicle_local_position.mat;
% constant value
RAD2DEG = 57.2957795;
DEG2RAD = 0.0174533;
% throttle when UAV is hovering, DJI F450
THR_HOVER = 0.527;

%% control parameter
KP_PITCH_ANGLE = Pixhawk_CSC.Parameter({single(4.0), 'KP_PITCH_ANGLE'});
KP_PITCH_RATE = Pixhawk_CSC.Parameter({single(0.1), 'KP_PITCH_RATE'});
KI_PITCH_RATE = Pixhawk_CSC.Parameter({single(0.045), 'KI_PITCH_RATE'});
KD_PITCH_RATE = Pixhawk_CSC.Parameter({single(0.0008), 'KD_PITCH_RATE'});
KP_ROLL_ANGLE = Pixhawk_CSC.Parameter({single(4.0), 'KP_ROLL_ANGLE'});
KP_ROLL_RATE = Pixhawk_CSC.Parameter({single(0.1), 'KP_ROLL_RATE'});
KI_ROLL_RATE = Pixhawk_CSC.Parameter({single(0.05), 'KI_ROLL_RATE'});
KD_ROLL_RATE = Pixhawk_CSC.Parameter({single(0.0008), 'KD_ROLL_RATE'});
KP_YAW_RATE = Pixhawk_CSC.Parameter({single(0.12), 'KP_YAW_RATE'});
KI_YAW_RATE = Pixhawk_CSC.Parameter({single(0.01), 'KI_YAW_RATE'});
KD_YAW_RATE = Pixhawk_CSC.Parameter({single(0.0005), 'KD_YAW_RATE'});

% KPXP = Pixhawk_CSC.Parameter({single(1), 'KPXP'});
% KPYP = Pixhawk_CSC.Parameter({single(1), 'KPYP'});
% KPZP = Pixhawk_CSC.Parameter({single(1), 'KPZP'});
% KVXP = Pixhawk_CSC.Parameter({single(1.4), 'KVXP'});
% KVXI = Pixhawk_CSC.Parameter({single(0.2), 'KVXI'});
% KVXD = Pixhawk_CSC.Parameter({single(0.05), 'KVXD'});
% KVYP = Pixhawk_CSC.Parameter({single(1.4), 'KVYP'});
% KVYI = Pixhawk_CSC.Parameter({single(0.2), 'KVYI'});
% KVYD = Pixhawk_CSC.Parameter({single(0.05), 'KVYD'});
% KVZP = Pixhawk_CSC.Parameter({single(0.5), 'KVZP'});
% KVZI = Pixhawk_CSC.Parameter({single(0.12), 'KVZI'});
% KVZD = Pixhawk_CSC.Parameter({single(0), 'KVZD'});

% integral saturation
Saturation_I_RP_Max = 0.3;
Saturation_I_RP_Min = -0.3;
Saturation_I_Y_Max = 0.2;
Saturation_I_Y_Min = -0.2;
Saturation_I_ah = 3.43;
Saturation_I_az = 5;

% 机体Z轴与惯性坐标系Z轴的最大夹角
MAX_ANGLE = pi / 4;
% max control angle, default 35deg
MAX_CONTROL_ANGLE_ROLL = 35;
MAX_CONTROL_ANGLE_PITCH  = 35;
% max control angle rate, rad/s 
MAX_CONTROL_ANGLE_RATE_PITCH = 3.8;
MAX_CONTROL_ANGLE_RATE_ROLL = 3.8;
MAX_CONTROL_ANGLE_RATE_YAW = 3.5;
% max control speed, m/s
MAX_CONTROL_VELOCITY_XY = 5;
MAX_CONTROL_VELOCITY_Z = 3;
% throttle amplitude
MAX_MAN_THR = 0.9;
MIN_MAN_THR = 0.05;

% 倾转分离的yaw权重
YAW_WEIGHT = 0.3;